#pragma once
#include "opencv2/opencv.hpp"
#include<iostream>
#include<fstream>
#include<chrono>
#include<vector>
#include <opencv2/opencv.hpp>
#include"../General/General.h"


namespace rm
{

class Odometry
{
public:
    Odometry()
    {
        time = 0;
        self_gimbal_pitch = 0;
        self_gimbal_yaw = 0;
        enemy_num = 0;
        enemy_distance = 0;
        enemy_angle = cv::Vec2f(0, 0);
        enemy_direction = cv::Point3f(0, 0, 1);
    }
    ~Odometry() {}

    const Odometry& operator =(const Odometry left)
    {
        this->time = left.time;
        this->self_gimbal_pitch = left.self_gimbal_pitch;
        this->self_gimbal_yaw = left.self_gimbal_yaw;
        this->enemy_num = left.enemy_num;
        this->enemy_distance = left.enemy_distance;
        this->enemy_angle = left.enemy_angle;
        this->enemy_direction = left.enemy_direction;
        return *this;
    }

    void print()
    {
        std::ofstream out("/home/nuc/pose.txt", std::ios::app);
        /*std::c*/out<<time<<" "<<self_gimbal_pitch<<" "<<self_gimbal_yaw<<" "<<enemy_num<<" "<<enemy_distance<<" "<<enemy_angle<<" "<<enemy_direction<<std::endl;
    }

    float time;

    float self_gimbal_pitch;
    float self_gimbal_yaw;

    int enemy_num;
    float enemy_distance;
    cv::Vec2f enemy_angle;
    cv::Point3f enemy_direction;

};



class Predictor   //有数字识别再用
{
public:
    Predictor()
    {
        queue_size = 16;

        move_module_r = STAYSTILL;
        move_module_t = STAYSTILL;
        rotate_module = STAYSTILL;

        delta_pitch_predict = 0;
        delta_yaw_predict = 0;
        delta_pitch = 0;
        delta_yaw = 0;

        bullet_speed = 29;
        max_change_angle = 5;
        max_change_distance = 0.5;
        max_change_time = 0.2;

        sentry_predict_done = false;
        pre_v_t = 0;
    }
    ~Predictor() {}

    void push(float time, float self_gimbal_pitch, float self_gimbal_yaw, int enemy_num, float enemy_distance, cv::Vec2f enemy_angle, cv::Point3f enemy_direction);
    void setBulletSpeed(float v);
    void refresh();
    void predict();
    void fit(int type, int n);
    void print();
    cv::Vec2f getCompensate()
    {
        return cv::Vec2f(delta_pitch, delta_yaw);
    }

    enum MovementModule
    {
        STAYSTILL,
        VELOCITYUNCHANGE,
        VELOCITYACCELERATING,
        OMEGAUNCHANGE,
        OMEGAACCELERATING,
        SENTRY
    };

    enum MoveType
    {
        RADIAL,
        TANGENTIAL,
        ROTATE
    };
    float a_r, v_r, a_t, v_t, theta, omega, alpha;
private:

    std::vector<Odometry> odometrys;
    int queue_size;
    float bullet_speed;

    std::chrono::high_resolution_clock::time_point t1, t2;
    int armorflag;

    int move_module_r;   //目标径向运动模式
    int move_module_t;   //目标切向运动模式
    int rotate_module;   //目标旋转模式

    //float a_r, v_r, a_t, v_t, theta, omega, alpha;

    float delta_pitch_predict;
    float delta_yaw_predict;

    float delta_pitch;
    float delta_yaw;

    float max_change_angle;
    float max_change_distance;
    float max_change_time;

    bool sentry_predict_done;
    double pre_v_t;
};
}
